"""
运动学计算模块

该模块实现了六足机器人的运动学计算，包括：
- 正运动学：根据关节角度计算机器人末端位置
- 逆运动学：根据末端位置计算关节角度
"""

import math
from typing import List, Tuple


class Kinematics:
    """运动学计算类"""

    # 机器人参数
    C = 44.60  # 大腿长度
    F = 75.00  # 小腿长度
    T = 126.50  # 足端长度
    FACTOR = 180 / math.pi / 0.24  # 角度转换因子

    @staticmethod
    def forward_kinematics(leg: int, angles: List[float]) -> List[float]:
        """
        正运动学：根据关节角度计算足端位置

        Args:
            leg: 腿的编号(0-5)
            angles: 三个关节的角度[弧度]

        Returns:
            List[float]: 足端坐标[x, y, z]
        """
        # 将角度转换为弧度
        theta1, theta2, theta3 = angles

        # 计算足端位置
        if leg < 3:  # 前三条腿
            x = (C + F * math.cos(theta2) + T * math.cos(theta2 + theta3)) * math.cos(
                theta1
            )
            y = (C + F * math.cos(theta2) + T * math.cos(theta2 + theta3)) * math.sin(
                theta1
            )
            z = F * math.sin(theta2) + T * math.sin(theta2 + theta3)
        else:  # 后三条腿
            x = (C + F * math.cos(theta2) + T * math.cos(theta2 + theta3)) * math.cos(
                theta1
            )
            y = (C + F * math.cos(theta2) + T * math.cos(theta2 + theta3)) * math.sin(
                theta1
            )
            z = -(F * math.sin(theta2) + T * math.sin(theta2 + theta3))

        return [x, y, z]

    @staticmethod
    def inverse_kinematics(leg: int, position: List[float]) -> List[float]:
        """
        逆运动学：根据足端位置计算关节角度

        Args:
            leg: 腿的编号(0-5)
            position: 足端坐标[x, y, z]

        Returns:
            List[float]: 三个关节的角度[弧度]
        """
        x, y, z = position
        angles = []

        # 计算第一个关节角度
        theta1 = math.atan2(y, x)
        angles.append(theta1)

        # 计算第二个和第三个关节角度
        L = math.sqrt(x * x + y * y)
        temp = z * z + (L - Kinematics.C) * (L - Kinematics.C)
        ft = math.sqrt(temp)

        a = math.atan2(z, L - Kinematics.C)
        b = math.acos(
            (Kinematics.F * Kinematics.F + ft * ft - Kinematics.T * Kinematics.T)
            / (2 * Kinematics.F * ft)
        )
        theta2 = a + b
        angles.append(theta2)

        theta3 = math.acos(
            (ft * ft - Kinematics.F * Kinematics.F - Kinematics.T * Kinematics.T)
            / (2 * Kinematics.F * Kinematics.T)
        )
        angles.append(theta3)

        return angles

    @staticmethod
    def angles_to_servo_positions(leg: int, angles: List[float]) -> List[int]:
        """
        将关节角度转换为舵机位置

        Args:
            leg: 腿的编号(0-5)
            angles: 三个关节的角度[弧度]

        Returns:
            List[int]: 三个舵机的位置值
        """
        # 将弧度转换为角度
        angles_deg = [math.degrees(angle) for angle in angles]

        if leg < 3:  # 前三条腿
            servo_positions = [
                313 + angles_deg[0] * Kinematics.FACTOR,
                500 - angles_deg[1] * Kinematics.FACTOR,
                687 - angles_deg[2] * Kinematics.FACTOR - 5,
            ]
        else:  # 后三条腿
            servo_positions = [
                687 - angles_deg[0] * Kinematics.FACTOR,
                500 + angles_deg[1] * Kinematics.FACTOR,
                313 + angles_deg[2] * Kinematics.FACTOR + 5,
            ]

        return servo_positions

    @staticmethod
    def get_point(leg: int, angle: float) -> List[float]:
        """
        计算足端在平面上的位置

        Args:
            leg: 腿的编号(0-5)
            angle: 旋转角度[度]

        Returns:
            List[float]: 足端坐标[x, y, z]
        """
        angle = math.radians(angle)  # 角度转弧度
        R = 271.5  # 外圈半径
        RM = 232.5  # 内圈半径
        base_angle_FB = 0.9465  # 前后腿基准角度
        base_angle_M = 0.7853  # 中间腿基准角度

        if leg == 0:
            x = R * math.cos(base_angle_FB + angle) - 58.5
            y = R * math.sin(base_angle_FB + angle) - 120.0
        elif leg == 1:
            x = RM * math.cos(base_angle_M + angle) - 64.70
            y = RM * math.sin(base_angle_M + angle) - 64.70
        elif leg == 2:
            x = R * math.sin(base_angle_FB - angle) - 120.0
            y = R * math.cos(base_angle_FB - angle) - 58.5
        elif leg == 3:
            x = R * math.cos(base_angle_FB - angle) - 58.5
            y = R * math.sin(base_angle_FB - angle) - 120.0
        elif leg == 4:
            x = RM * math.cos(base_angle_M - angle) - 64.70
            y = RM * math.sin(base_angle_M - angle) - 64.70
        elif leg == 5:
            x = R * math.sin(base_angle_FB + angle) - 120.0
            y = R * math.cos(base_angle_FB + angle) - 58.5
        else:
            x = 100
            y = 100

        return [x, y, -70]  # 默认z坐标为-70
